# -*- coding: utf-8 -*-
import os
import threading
import rospy
import roslaunch
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import String
from sensor_msgs.msg import BatteryState

class robotsys():

    battery_ratio = 76
    liquid_ratio = 63
    speed_x = 0.0
    speed_y = 0.0
    speed_angle = 0.0
    pose_x = 0.0
    pose_y = 0.0

    def __init__(self):
        self.battery_sub = rospy.Subscriber("battery", BatteryState, self.battery_receive, queue_size=10)
        self.odom_sub = rospy.Subscriber("odom", Odometry, self.odom_receive, queue_size=10)
        self.__do_move = False
        self.__lock = threading.Lock()
        # threading.Thread(target=self.heartbeat)

    def battery_receive(self, msg):
        # rospy.loginfo('received battery message from ros : %s'%msg.data)
        self.battery_ratio = (msg.voltage*100)/5

    def odom_receive(self, msg):
        # rospy.loginfo('received message from ros : %s'%msg)
        robotsys.speed_x = msg.twist.twist.linear.x
        robotsys.speed_y = msg.twist.twist.linear.y
        robotsys.speed_angle = msg.twist.twist.angular.z
        robotsys.pose_x = msg.pose.pose.position.x
        robotsys.pose_y = msg.pose.pose.position.y

    def status(self,robot_status):
        data = {"method": "system.status", "code": "1",
                "msg": "", "status": robot_status }
        rospy.loginfo('robot_status-->'+str(robot_status))
        return data

    def speed(self):
        rospy.loginfo('speed_x : %s'%robotsys.speed_x)
        rospy.loginfo('speed_y : %s'%robotsys.speed_y)
        rospy.loginfo('speed_angle : %s'%robotsys.speed_angle)
        data = {"method": "system.speed", "code": "1", "msg": "",
                "speed_x": robotsys.speed_x, "speed_y": robotsys.speed_y, "speed_angle": robotsys.speed_angle}
        return data

    def pose(self):
        rospy.loginfo('pose_x : %s'%robotsys.pose_x)
        rospy.loginfo('pose_y : %s'%robotsys.pose_y)
        data = {"method": "system.pose", "code": "1", "msg": "","x": robotsys.pose_x, "y": robotsys.pose_y}
        return data
    
    def move(self, speed_x, speed_y, speed_angle):
        move_cmd = Twist()
        move_cmd.linear.x = speed_x
        move_cmd.linear.y = speed_y
        move_cmd.angular.z = speed_angle
        def move(self):
            self.__move(int(60* 0.2), move_cmd)
        return self._do_move(move)
    
    def __move(self, ticks, move_cmd):
        cmd_vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
        r = rospy.Rate(5)
        for t in range(ticks):
            if not self.__do_move:
                break
            cmd_vel_pub.publish(move_cmd)
            r.sleep()
        cmd_vel_pub.publish(Twist())

    def _do_move(self, callback):
        def func():
            if not callable(callback):
                return
                
            if self.__do_move:
                rospy.loginfo('now moving')
                return

            with self.__lock:
                self.__do_move = True

            callback(self)

            with self.__lock:
                self.__do_move = False
        thread = threading.Thread(target=func)
        thread.start()
        data = {"method": "system.move", "code": "1", "msg": ""}
        return data
        # return thread

    def shutdown(self):
        os.system("shutdown -h +1")
        data = {"method": "system.shutdown", "code": "1", "msg": ""}
        return data